/*
 * File - inudev_ros_nodelet.cpp
 *
 * This file is part of the Inuitive SDK
 *
 * Copyright (C) 2014-2020 Inuitive All Rights Reserved
 *
 */

/**
 * \cond INTERNAL
 */

#include "config.h"
#include "Version.h"

#include <nodelet/nodelet.h>
#include <pluginlib/class_list_macros.h>

/**
 * \endcond
 */

#include "inudev_ros_nodelet.h"

/**
 * \file inudev_ros_nodelet.cpp
 *
 * \brief ROS Nodelet
 */

/**
 * \cond INTERNAL
 */

static const char rcsid[]="$Id: 0ddcc688c69190eadb1d83914d0973b81cfb075e $";

/**
 * \endcond
 */

/**
 * \mainpage inudev_ros_nodelet synopsis
 *
 * \section intro_sec Introduction
 *
 * InuROS is a ROS nodelet consuming InuSW frames, publishing them on ROS topics via standard messages.
 * InuROS publishing is demand activated, meanning messages are published only on topics subscribed to,
 * and InuSW streams are only activated for subscribed topics
 *
 * InuROS's package name is inudev_ros_nodelet.
 *
 * \section setup Setup
 *
 * This section describes steps required regradless of use (i.e., delopement or execution)
 *
 * Developing and running inudev_ros_nodelet requires ROS "noetic" be installed.
 * Full installation instructions can be found in <a href="http://wiki.ros.org/noetic/Installation/Ubuntu">ROS Wiki</a>, specificaly:
 *
 * <ul>
 *   <li><a href="https://wiki.ros.org/noetic/Installation/Ubuntu">Ubuntu install of ROS noetic</a></li>
 *   <li><a href="https://wiki.ros.org/ROS/Tutorials/InstallingandConfiguringROSEnvironment">Installing and Configuring ROS Environment</a></li>
 *   <li><a href="https://wiki.ros.org/catkin/Tutorials/using_a_workspace">Building and using catkin packages in a workspace</a></li>
 * </ul>

 * repeated here for ease of use.
 *
 * \subsection ros_install Insalling ROS
 *
 * \code
 * sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list'
 * sudo apt install curl
 * curl -s https://raw.githubusercontent.com/ros/rosdistro/master/ros.asc | sudo apt-key add -
 * sudo apt update
 * sudo apt install ros-noetic-desktop-full
 * \endcode
 *
 * \subsection env Environment Setup
 *
 * \code
 * echo "source /opt/ros/noetic/setup.bash" >> ~/.bashrc
 * source ~/.bashrc
 * sudo apt install python-rosdep python-rosinstall python-rosinstall-generator python-wstool build-essential
 * sudo rosdep init
 * rosdep update
 * \endcode
 *
 * \subsection InuDev Installing InuDev
 *
 * Obtain and install an InuDev package (either from web source or file):
 * 
 * \code
 * wget https://<url to package>/inudev_4.13.0004.22-1_amd64.deb
 * sudo apt install -f inudev_4.13.0004.22-1_amd64.deb
 * \endcode
 *
 * \subsection inuros_install Installing InuROS
 *
 * Use the following command to install inudev_ros_nodelet from pre-built Debian package:
 * 
 * \code
 * dpkg -i ros-noetic-inudev-ros-nodelet_<version>-0focal_amd64.deb
 * \endcode
 *
 * The content of the resulting .deb can be viewed using:
 *
 * \code
 * dpkg --contents  ../ros-noetic-inudev-ros-nodelet_<version>-0focal_amd64.deb
 * \endcode
 *
 * In the future, InuROS may also be avaliable as ROS package:
 *
 * \code
 * sudo apt-get install ros-noetic-InuROS
 * \endcode
 *
 * \section Developement InuROS Devlopment/Build
 *
 * The following steps are required in order to establish InuROS developement environment.
 *
 * \subsection dev Setup development environment
 *
 * Install build tools:
 *
 * \code
 * sudo apt update
 * sudo apt install build-essential
 * sudo apt-get install manpages-dev
 * \endcode
 *
 * Version 3.23 of cmake is required. Install as follows:
 *
 * Warning: DO NOT USE "sudo apt purge --auto-remove cmake" to remove the existing pakage!
 *
 * \code
 * wget -O - https://apt.kitware.com/keys/kitware-archive-latest.asc 2>/dev/null | gpg --dearmor - | sudo tee /etc/apt/trusted.gpg.d/kitware.gpg >/dev/null
 * sudo apt-add-repository 'deb https://apt.kitware.com/ubuntu/ focal main'
 * sudo apt update
 * sudo apt install cmake
 * \endcode
 *
 * \subsection prepare_doc Documentation build requirments
 *
 * Install rosdoc_lite using:
 * \code
 * sudo apt-get install ros-noetic-rosdoc-lite
 * \endcode
 *
 * Install LaTeX using:
 * \code
 * sudo apt-get install texlive-full
 * \endcode
 *
 * \subsection ros_env Create ROS Workspace
 *
 * \code
 * mkdir -p ~/catkin_ws/src
 * cd ~/catkin_ws/
 * export INUROS_DOC=$PWD/src/inudev_ros_nodelet/doc
 * catkin_make
 * \endcode
 *
 * \subsection se1 Setting up source environment 
 *
 * InuROS source is avaliable via bitbucket, either using username/password or via SSH public key.
 *
 * Pull <i>InuROS</i> from git (using username + app password):
 * 
 * \code
 * cd ~/catkin_ws/src
 * git clone https://<user>@bitbucket.org/inuitive/inuros.git inudev_ros_nodelet
 * \endcode
 *
 * Or, using SSH (Public/Private SSH Keys must have been generated and installed, on both the server and the client,
 * consult <a href="https://support.atlassian.com/bitbucket-cloud/docs/clone-a-git-repository/">here</a> for details):
 * 
 * \code
 * cd ~/catkin_ws/src
 * git clone git@bitbucket.org:inuitive/inuros.git inudev_ros_nodelet
 * \endcode
 *
 * Install vision_msgs package as follows:
 *
 * \code
 * sudo apt-get install ros-noetic-vision-msgs
 * \endcode
 *
 * \subsection inudev_env Set InuDev path for InuROS
 *
 * InuROS build assumes InuDev is installed in /opt/Inuitive . Path used may be overriden by setting certian environment
 * variables (only required if building from source.) Should it be required, add the folloing to the environmet
 * (via ~/.bashrc):
 *
 * \code
 * export INUITIVE_STREAMS_LIBS=/opt/Inuitive/InuDev/lib
 * export INUITIVE_STREAMS_INCLUDE=/opt/Inuitive/InuDev/include
 * export INUITIVE_COMMON_INCLUDE=/opt/Inuitive/InuDev/include/Common
 * \endcode
 *
 * \subsection build Building inudev_ros_nodelet
 *
 * \code
 * catkin_make catkin_make -DCMAKE_VERBOSE_MAKEFILE=OFF -DCMAKE_BUILD_TYPE=Debug -DCATKIN_WHITELIST_PACKAGES="inudev_ros_nodelet" install
 * \endcode
 *  
 * \subsection run_everything_dev Run everything (Development)
 *
 * \code
 * source ./devel/setup.sh (or source ./install/setup.sh, if running from installation)
 * export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:/opt/Inuitive/InuDev/lib
 * roslaunch src/inudev_ros_nodelet/launch/inudev_ros_nodelet.launch
 * \endcode
 *
 * Depending on args set in the launch file, various ROS nodes will start:
 *
 * - rqt_console
 * - inudev_ros_nodelet
 * - rqt_gui
 * - static_transform_publisher
 * - rqt_image_view
 * - rviz
 * - image_view
 *
 * \subsection ints Installation
 *
 * InuROS package can be installed either locally or globally.
 *
 * Install locally using the following command:
 *
 * \code
 * catkin_make install
 * \endcode
 *
 * Install globally using the following command:
 *
 * \code
 * catkin_make -DCMAKE_INSTALL_PERFIX=/opt/ros/noetic install
 * \endcode
 *
 * \subsection run_everything_dep Run everything (Deployment)
 *
 * \code
 * source /opt/ros/noetic/setup.sh
 * export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:/opt/Inuitive/InuDev/lib
 * roslaunch /opt/ros/noetic/share/inudev_ros_nodelet/launch/inudev_ros_nodelet.launch
 * \endcode
 *
 * \subsection terminate Terminate
 *
 * Issue the following command (from another console):
 *
 * \code
 * rosrun nodelet nodelet unload /inudev_ros_nodelet nodelet_manager
 * \endcode
 *
 * \subsection imu Display IMU data
 *
 * IMU data can be viewed by adding
 * \code
 * /sensor_msgs/Imu/angular_velocity/{x,y,z}
 * \endcode
 * and
 * \code
 * /sensor_msgs/Imu/linear_acceleration.{x,y,z}
 * \endcode
 * to rqt_plot
 *
 * \subsection point_cloud Point cloud viewer
 *
 * - run rviz [rosrun rviz rviz]
 * - Add display type in rviz [Add->PointCloud2] or may be just PointCloud
 * - play your bag file [rosbag play abcdefg.bag]
 * Browse for the topic in the added display type in rviz
 * 
 * \subsection doc Documentation
 *
 * Documentation (using <a href="http://wiki.ros.org/rosdoc_lite">rosdoc_lite</a>; work-in-progress):
 * 
 * \code
 * rosdoc_lite -o new_doc .
 * \endcode
 * 
 * \subsection packaging_prepare Packaging Preperation
 *
 * Taken from <a href="https://docs.ros.org/en/foxy/How-To-Guides/Building-a-Custom-Debian-Package.html">ROS Wiki, Building a custom Debian package</a>.
 *
 * The following steps are required in prepation for .deb package generation.
 *
 * Install bloom-generate, part of python-bloom package, as well as fakeroot, using:
 *
 * \code
 * apt-get install python-bloom fakeroot
 * \endcode
 *
 * cd to the package directory, then invoke: <b>These steps are not required unless inuros_dev_nodelet package content changes</b>.
 *
 * \code
 * cd src/inuros_dev_nodelet
 * perl updateInuROSversion_Linux.pl . 1.0.17.3
 * rm -rf debian
 * bloom-generate rosdebian --os-name ubuntu --os-version focal --ros-distro noetic
 * \endcode
 *
 * as well as debian directory (already exists in the package.)
 *
 * \subsection packaging Packaging
 *
 * Prepare to build the .deb package by verifying the content of LD_LIBRARY_PATH environmt variable, as follows (should only 
 * be performed once per login):
 *
 * \code
 * export LD_LIBRARY_PATH=/opt/Inuitive/InuDev/lib:/opt/Inuitive/InuDev/bin:$LD_LIBRARY_PATH
 * \endcode
 *
 * Build .deb package as follows:
 *
 * \code
 * dh_clean
 * fakeroot make --no-print-directory -f debian/rules binary
 * \endcode
 *
 * This will produce:
 *
 * \code
 * ros-noetic-inudev-ros-nodelet_<version>-0focal_amd64.deb
 * ros-noetic-inudev-ros-nodelet-dbgsym_<version>-0focal_amd64.ddeb
 * \endcode
 * one directory up.
 *
 * The content of the .deb package can be viewed using:
 *
 * \code
 * dpkg -c ../ros-noetic-inudev-ros-nodelet_<version>-0focal_amd64.deb
 * \endcode
 *
 * \subsection dyn_config Dynamic Configuration
 *
 * inudev_ros_nodelet includes (rudimentary and preleminary) support for <a href="http://wiki.ros.org/dynamic_reconfigure">dynamic confiuration</a>. 
 *
 * Paramsters include:
 * 
 * - video_verbose
 * - depth_verbose
 * - disparity_verbose
 * - pointcloud_verbose
 * - webcam_verbose
 * - fisheye_verbose  
 * - slam_verbose
 * - imu_verbose
 * - features_verbose
 *
 * inudev_ros_nodelet's configuration may be examined and modified using <a href="http://wiki.ros.org/rqt_reconfigure">rqt_reconfigure</a> or 
 * using dynamic_reconfigure nodelet, as follows:
 *
 * Installation:
 *
 * \code
 * rosdep install dynamic_reconfigure
 * \endcode
 *
 * Usage:
 *
 * \code
 * \endcode
 *
 * \subsection changelog Changelog
 *
 * Configured values are save in the cfg directory.
 *
 * \section test Testing
 *
 * inudev_ros_nodelet is tested using gtest framework.
 *
 * \subsection build_gtest Building + running tests
 *
 * Use the following catkin_make invocation to compile and run gtest:
 *
 * \code
 * catkin_make run_tests_inudev_ros_nodelet_gtest
 * \endcode
 *
 * \subsection results Running and viewing gtest results
 *
 * \code
 * catkin_test_results build/test_results
 * \endcode
 *
 * Also:
 *
 * \code
 * rosrun inudev_ros_nodelet inudev_ros_nodelet-test
 * \endcode
 * 
 * or even (list gtests):
 * 
 * \code
 * rosrun inudev_ros_nodelet inudev_ros_nodelet-test --gtest_list_tests
 * \endcode
 *
 * See <a href="http://docs.ros.org/noetic/api/catkin/html/howto/format2/run_tests.html">here</a> for more info.
 *
 * \subsection stopping Stopping gtest
 *
 * When ROS gtest runs as a nodelet, it is possible to stop it using:
 *
 * \code
 * rosrun nodelet nodelet unload /tester nodelet_manager
 * \endcode
 * 
 * If ROS gtest runs as node, use:
 * 
 * \code
 * rosnode kill /tester
 * \endcode
 *
 */


PLUGINLIB_EXPORT_CLASS(__INUROS__NAMESPACE__::CInuDevRosNodelet, nodelet::Nodelet)

namespace __INUROS__NAMESPACE__
{
    int CInuDevRosNodelet::mVerbose = 0;
    inudev_ros_nodelet::inudev_ros_nodeletConfig CInuDevRosNodelet::config;

#if POINTCLOUD_DUMP_FRAME
    int CInuDevRosNodelet::mLogPointcloud = 0;
#endif

    CInuDevRosNodelet::CInuDevRosNodelet()
        : fps(10.0)
        , mRosSensor(nullptr)
    {
        NODELET_INFO_STREAM(__INUROS_FUNCTION_NAME__ << ": " << getName());
    }

    CInuDevRosNodelet::~CInuDevRosNodelet()
    {
        NODELET_INFO_STREAM(__INUROS_FUNCTION_NAME__ << ": " << getName());
    }

    void CInuDevRosNodelet::callback(inudev_ros_nodelet::inudev_ros_nodeletConfig &newConfig, uint32_t level)
    {
        config = newConfig;
        if(mRosSensor->getSensor() != nullptr)
        {
            mRosSensor->getSensor()->SetProjectorLevel((InuDev::EProjectorLevel)config.Projector_Power, InuDev::EProjectors::ePatterns);
            InuDev::CSensorControlParams AEParams;
            AEParams.AutoControl = config.Auto_Sensor_Control;
            AEParams.AnalogGain = config.Gain;
            AEParams.ExposureTime = config.Exposure;
            AEParams.Params.UseROI = config.Use_ROI;
            AEParams.Params.ROITopLeft = InuDev::CPoint<2, unsigned int>(config.ROI_Top_Left_x, config.ROI_Top_Left_y);
            AEParams.Params.ROIBottomRight = InuDev::CPoint<2, unsigned int>(config.ROI_Bottom_Right_x, config.ROI_Bottom_Right_y);
            mRosSensor->getSensor()->SetSensorControlParams(AEParams, config.Sensor_ID);
        }
    }

    void CInuDevRosNodelet::onInit()
    {
        NODELET_INFO_STREAM(__INUROS_FUNCTION_NAME__ << ": " << getName());
        NODELET_INFO_STREAM(__INUROS_FUNCTION_NAME__ << ": Version " << version);

        /* node handles */
        /* See https://answers.ros.org/question/32061/what-are-the-differences-between-getnodehandle-and-getprivatenodehandle-in-nodelet/ */

        privNh = getMTPrivateNodeHandle();
        node = getMTNodeHandle();

        /* Command line parametrs (less ROS and nodelet specific argumments) */

        const std::vector<std::string> &myArgs(getMyArgv());

        NODELET_INFO_STREAM(__INUROS_FUNCTION_NAME__ << ": Num of command line arguments = " << myArgs.size());

        for (int i=0;i<myArgs.size();i++)
        {
            NODELET_INFO_STREAM(__INUROS_FUNCTION_NAME__ << ": arg[" << i << "]=\"" << myArgs.at(i) << "\"");
        }

        /* get info */

        std::string name = privNh.getUnresolvedNamespace();
        int pos = name.find_last_of('/');
        name = name.substr(pos + 1);

        NODELET_INFO_STREAM(__INUROS_FUNCTION_NAME__ << ": Initialising nodelet publishers... [" << name << "]");

        /* params */

        privNh.param<int>("verbose", mVerbose, 0);
        NODELET_INFO_STREAM(__INUROS_FUNCTION_NAME__ << ": param(verbose)=" << mVerbose);

#if POINTCLOUD_DUMP_FRAME
        privNh.param<int>("log_pointcloud", mLogPointcloud, 4444);
        NODELET_INFO_STREAM(__INUROS_FUNCTION_NAME__ << ": param(log_pointcloud)=" << mLogPointcloud);
#endif

        /* sensor */

        mRosSensor = std::make_shared<CRosSensor>(node, fps, "sensor");
        NODELET_INFO_STREAM(__INUROS_FUNCTION_NAME__ << ": Nodelet onInit done.");
        //mRosSensor->getSensor()->SetProjectorLevel(InuDev::EProjectorLevel::eHigh, InuDev::EProjectors::ePatterns);

        /* dynamic reconfigure */

        dynamic_reconfigure::Server<inudev_ros_nodelet::inudev_ros_nodeletConfig>::CallbackType f;
        f = std::bind(&CInuDevRosNodelet::callback, this, std::placeholders::_1, std::placeholders::_2);

        // Initialize configuration default from declared
        config = inudev_ros_nodelet::inudev_ros_nodeletConfig::__getDefault__();

        server = std::make_shared<dynamic_reconfigure::Server<inudev_ros_nodelet::inudev_ros_nodeletConfig>>(privNh);
        server->setCallback(f);
    }
}

